package com.guazi.trackingtest.cal.op;

import com.guazi.trackingtest.cal.data.Point3D;

public class CalculateLAcc {
    public static Point3D calculateDisplacement(Point3D lastVelocity, Point3D velocity, double timeInterval) {
        double sx = formula(timeInterval, lastVelocity.x, velocity.x);
        double sy = formula(timeInterval, lastVelocity.y, velocity.y);
        double sz = formula(timeInterval, lastVelocity.z, velocity.z);

        return new Point3D(sx, sy, sz);
    }

    public static Point3D calculateVelocity(Point3D lastVelocity, Point3D lastAcceleration, Point3D acceleration, double timeInterval) {
        Point3D velocity = new Point3D(lastVelocity);

        double vx = formula(timeInterval, lastAcceleration.x, acceleration.x);
        double vy = formula(timeInterval, lastAcceleration.y, acceleration.y);
        double vz = formula(timeInterval, lastAcceleration.z, acceleration.z);

        return velocity.add(new Point3D(vx, vy, vz));
    }

    public static Point3D calculatePosition(Point3D position, Point3D displacement) {
        Point3D newPosition = new Point3D(position);

        return newPosition.add(displacement);
    }

    private static double formula(double t, double v0, double vt) {
        return 0.5 * t * (v0 + vt);
    }
}
